Optimized Dynamic Collision Avoidance Algorithm for USV Path Planning

Ship collision avoidance is a complex process that is influenced by numerous factors. In this study, we propose a novel method called the Optimal Collision Avoidance Point (OCAP) for unmanned surface vehicles (USVs) to determine when to take appropriate actions to avoid collisions. The approach combines a model that accounts for the two degrees of freedom in USV dynamics with a velocity obstacle method for obstacle detection and avoidance. The method calculates the change in the USV’s navigation state based on the critical condition of collision avoidance. First, the coordinates of the optimal collision avoidance point in the current ship encounter state are calculated based on the relative velocities and kinematic parameters of the USV and obstacles. Then, the increments of the vessel’s linear velocity and heading angle that can reach the optimal collision avoidance point are set as a constraint for dynamic window sampling. Finally, the algorithm evaluates the probabilities of collision hazards for trajectories that satisfy the critical condition and uses the resulting collision avoidance probability value as a criterion for course assessment. The resulting collision avoidance algorithm is optimized for USV maneuverability and is capable of handling multiple moving obstacles in real-time. Experimental results show that the OCAP algorithm has higher and more robust path-finding efficiency than the other two algorithms when the dynamic obstacle density is higher.


Introduction
Ship collision is an imperative task for navigation safety at sea [1]. Unmanned surface vehicles (USVs) have gained significant attention in recent years due to their potential for various applications such as oceanographic research, environmental monitoring, and maritime security [2]. However, the increasing use of USVs also raises concerns about the safety of navigation, especially when operating in crowded environments [3]. Collision avoidance is a critical issue that needs to be addressed to ensure safe and efficient navigation of USVs. A USV's collision avoidance algorithm can be considered a local path-planning algorithm. This paper focuses on local path-planning methods. Many local path-planning algorithms have been reported for collision avoidance against static and dynamic obstacles [4,5]. By now, many obstacle avoidance algorithms have been proposed by international scholars, all of which rely more or less on global path planning and mapping, such as a bug algorithm [6], a vector field histogram method [7], and an artificial potential field method [8]. Many improved heuristic algorithms have also been studied for local path planning. For instance, a hybrid adaptive path-planning scheme based on global path planning and local dynamic collision avoidance for unmanned surface vehicles under complex marine environments was proposed in [9]. This method systematically considers the impact of waves and currents on the navigation of USVs. In recent years, some scholars have emphasized the dynamic collision avoidance of ships by incorporating methods such as reinforcement learning and USVs is considered and a detailed algorithm flow and design are presented. In Section 5, the results of computational experiments performed for the evaluation of the proposed algorithm are presented. According to simulation experiments, we compare the effects of three different algorithms on the collision avoidance path selection of a USV and analyze the degree of excellence resulting from the influence of various collision avoidance factors in path selection. Finally, in Section 6, the conclusions are discussed. Figure 1 shows a schematic description of course angles, heading angles, and sideslip angles of the USV dynamic model. In this coordinate frame, x i and y i represent north and east directions, while υ and ν are the divisions of V on the x 0 -and y 0 -axes, respectively [37,38]. From Figure 1, we can see that the sideslip angle β can be calculated as β = sin −1 υ V , the course angle θ can be defined using the heading angle (yaw) ψ, and the sideslip angle is θ = ψ + β [39,40]. The equations of the USV motion state can be represented as:

USV Dynamic Model
where x and y represent the position coordinates of the USV, V represents the velocity of the USV, ψ represents the heading angle of the USV, and ω represents the angular velocity of the USV. The equations of the obstacle motion state can be represented as: where x o and y o represent the position coordinates of the obstacle, v o represents the velocity of the obstacle, and ψ o represents the heading angle of the obstacle.
In this paper, we present a four-layered control structure in Figure 2, which consists of context awareness, behavioral decision-making, the obstacle avoidance algorithm, and executive control. In this study, we describe a USV marine collision avoidance strategy by the self-discipline method, t cpa stands for the time to the closest point of approach, and d cpa stands for the distance to the closest point of approach [41].
To support the collision avoidance of USVs in complex environments, this paper proposes a dynamic model that takes into account factors such as the mass, velocity, acceleration, water resistance, propulsive force, and gravitational acceleration of the USV in order to predict the changes in the position and velocity of the USV over time.

Recognition of Collision Avoidance Situations
The closest point of approach (CPA) is the point at which the distance between the ship and another target object will reach its minimum value. The geometry of the distance at CPA is illustrated in Figure 3; r is the Euclidean distance between the center points of the two ships. The equation describing the distance between the USV and obstacles can be represented as: CPA consists of two parameters: the distance at the closest point of approach (d cpa ) and the time to the closest point of approach (t cpa ); αis the angle between the relative bearing of the obstacle ship and the heading of the USV [41,42].

Safety Threshold d sa f e Establishment
In this study, we set a safety threshold d sa f e to evaluate the risk of a collision between vessels. A crash may occur whenever d cpa falls below this safety threshold. A pathfinding algorithm based on this criterion readjusts the parameters in the path trajectory evaluation function when the distance at the closest point of approach (CPA) is below d sa f e , which is given by where γ is the range between the two vessels andṙ = −v r cos η is the range rate. Here, η is the incident angle and v r denotes the relative velocity of the two vessels. If d cpa = 0, it expresses that after the two ships have sailed for the t cpa period, if they do not change their track, the two vessels will inevitably collide; d cpa > 0 indicates that the target ship passes by the USV's bow; d cpa < 0 indicates that the target ship passes the USV's stern [43]. However, the collision risk of the ship is not limited to d cpa = 0: the collision risk also considers t cpa . A larger t cpa indicates that it will take a long time to reach the nearest encounter distance and the degree of urgency is not high. When t cpa is small, it shows that the ship encounter will occur immediately and the situation is more dangerous [44].

Collision Avoidance Urgency Identification
This paper establishes the optimal collision avoidance velocity and the latest steering selection time for USVs based on the velocity obstacle method. At the same time, it provides a sufficient time-space margin for avoiding obstacles using the information of relative position and relative speed and the extensibility of the relative speed vector in space-time information.
As illustrated in Figure 4, circle O is a dynamic obstacle, and gray shading ξ all denotes the maximum explored space of all feasible domains of linear and angular velocities obtained by using the velocity window of the DWA as the USV passes through point A. Point B is the optimal collision avoidance point. Sector area ξ path is the velocity window after t cpa time as the feasible area of the USV, which is denoted as the blue sector area. Sector area ξ path ξ all is the optimal area of the USV driving area; the focus of this paper is to obtain the optimal driving area after screening the existing DWA velocity sampling window.
where V A is the current speed of the USV and V r is the relative speed of the USV relative to the incoming ship. In the optimal feasible area, the USV determines its optimal travel directly through the joint adjustment of different evaluation factors in the following formula: In Equation (8), w 1 , w 2 , w 3 , and w 4 are four collision risk coefficients that represent the impact of different factors on the urgency of obstacle avoidance by the USV. These weight coefficients can be adjusted according to specific situations to better adapt to different obstacle avoidance scenarios; g 1 (d), g 2 (v), g 3 (ψ), and g 4 (∇) are function expressions of different factors representing the impact of the distance between the USV and the obstacle, the velocity of the USV, and the direction of the USV on the urgency of obstacle avoidance.

Generation of Dynamic Obstacle Avoidance Points
To determine the optimal avoidance point that is dynamically generated based on the current position, velocity, and direction of the USV as well as the obstacle information obtained from sensors, it is necessary to compute the obstacle avoidance urgency using the function f (d, v, ψ, ∇) as defined in Equation (8). The velocity and direction of the USV are adjusted based on the obstacle avoidance urgency and dynamically generated best avoidance point. As the USV moves and encounters new obstacles, the obstacle avoidance urgency and dynamically generated best avoidance points are continuously updated to ensure safe and effective obstacle avoidance.
Algorithm 1 shows the pseudo-code to generate the optimal collision avoidance point for path candidates.
The generation of the optimal collision avoidance decision point is mainly based on the velocity obstacle method, and the optimal collision avoidance angle is added to the generated nodes (CPA) as an expanded search range. The traditional VOM algorithm uses the cost function of path length in dynamic obstacle avoidance, which can make the generated path shortest; however, the shortest-path optimization index may make the initial path close to the obstacle and increase the collision risk when the unmanned ship is driving. To solve this problem, this paper adopts the weighted cost function pair of integrated path length and optimal steering collision avoidance angle to generate the best collision avoidance decision point to ensure the safety of unmanned ship driving.

Selection and Evaluation of Obstacle Avoidance Points
In Equation (8), the w 1 , w 2 , w 3 , and w 4 weight coefficients can be adjusted according to specific situations to better adapt to different obstacle avoidance scenarios; g 1 (d), g 2 (v), g 3 (ψ), and g 4 (∇) are four function expressions that can be designed and adjusted according to the actual situation to more accurately reflect the impact of different factors on the urgency of obstacle avoidance.

•
Distance evaluation: Calculates the distance ratio between the minimum encounter distance and the safety threshold under the current USV state. The smaller the ratio, the safer it is.
• Velocity evaluation: Evaluates the ratio of the time taken to reach the d sa f e location to the t cpa at the current speed of the USV. The smaller the ratio, the safer it is.
• Direction angle evaluation: Evaluates the steering angle between the USV coordinates and the CPA coordinates. The larger the steering angle, the greater the risk.
• Direction angle evaluation: The symbol ∇ represents the minimum gradient, and g 4 (∇) is the function that describes the effect of the minimum gradient on the obstacle avoidance urgency level. In this paper, the minimum gradient represents the rate of change in the distance between the USV and the obstacle, which can be used to evaluate the dynamic relationship between the USV and obstacle and the approach speed.
The idea of the OCAP algorithm is based on the traditional DWA and is combined with the optimal collision avoidance point content in the previous section. First, the increments of vessel linear velocity and heading angle (ν, ψ) that can reach the optimal collision avoidance point are set as the constraints for dynamic window sampling. Secondly, based on the constraints, the algorithm calculates the critical conditions for the USV avoidance action. Finally, the algorithm evaluates the collision hazard risk probability on the trajectory formed by the optimized velocity region. It uses the collision avoidance probability value as an evaluation criterion to assess the merit of the route. The evaluation result selects the corresponding optimal velocity command (ν, ψ). The sampled data meet the optimal timing of collision avoidance and satisfy the collision avoidance rules to accomplish real-time obstacle avoidance and fast driving tasks in complex dynamic scenarios.

Search for Optimal Obstacle Avoidance Points
The primary focus of this chapter involves a three-step approach. Firstly, utilizing traditional speed sampling, a collision risk assessment is carried out by calculating the safe distance (d sa f e ) within the current speed window. Secondly, within the dynamic window, unsafe areas are excluded and the velocity obstacle is utilized to determine the optimal speed sample that satisfies the requirements of collision avoidance at sea. Lastly, the steering strategy of the USV is optimized to ensure that the optimized path aligns with the collision avoidance rules at sea.
Velocity and maximum acceleration limit minimum safety domain based on d sa f e : Line AO is the connection between point A and point O, m and l are the two tangents of the obstacle safety expansion circle, π 2 − ψ is the heading angle of the USV, and β − π 2 is the heading angle of the incoming obstacle vessel, as shown in Figure 5. The dashed triangle represents the velocity vector triangle after the active collision avoidance behavior taken by the USV. V A is the velocity after steering, ∆ψ is the steering angle of the USV, ∆η is the steering angle of V r , and || AP || 2 is the shortest encounter distance between the ship and the incoming ship after a period of time after the ship has performed collision avoidance behavior (such as steering or speed change). It is obvious that in the controllable speed sampling window, a larger d cpa means a lower collision hazard probability. As long as abs(η) ≥ µ exists at any time-that is, the sum velocity vector is located outside the triangle formed by m and l-the USV will not collide with the dynamic obstacle (shaded area shown in Figure 5).
The velocity of the USV is (V A , ψ), the velocity of the target obstacle is (V o , β), the speed of the USV relative to the obstacle is V r , the relative angle can be expressed as: ψ = ∠(X, V a ), θ = ∠(X, AO), λ = ∠(X, V r ), ϕ = ∠(V r , V A ), µ = ∠(AO, m) = ∠(AO, l). It can be seen from Figure 5 that the velocity triangle is composed of v A , v o , and v r .
Geometric relationships between d cpa and t cpa are shown in the following equations.
By obtaining speed V o and angle β of the obstacle using the sensor in advance, the USV can adjust speed V A and angle ψ in advance to avoid the obstacle in order to change the angle to meet abs(η) ≥ µ.
According to the velocity obstacle method, the differential of the yaw angle is used to express the attitude change rate of the USV, i.e., the change rate of the attitude angle, as shown in Equation (13). Therefore, using Equations (14)-(16), we can calculate the derivative of the adjustment variable as follows: The difference of yaw angle is used to express the attitude change amount of the USV, i.e., the change amount of the attitude angle, as shown in Equations (17) and (18). These differential and difference values are crucial parameters in the control model of unmanned ships, as they directly affect the motion state and control the performance of the USV. By utilizing the differential and difference of the yaw, the control system can calculate control commands to adjust the attitude angles of the USV, thereby enabling it to maintain stable navigation or perform specific tasks, such as obstacle avoidance or search and rescue operations.
From Figure 5, we can see that the total angle after the relative speed is turned is ∆η + η; where abs(η + ∆η) ≥ µ, the USV can complete the obstacle avoidance. Figure 5. Improved velocity obstacle method and d cpa .

Design of the Algorithm Framework
The algorithm framework as shown in Figure 6, mainly involves three modules: dynamic sampling, dynamic obstacle avoidance, and an obstacle avoidance point-set optimization algorithm. Among them, dynamic sampling is used to determine the current optimal obstacle avoidance area, dynamic obstacle avoidance is used to calculate the optimal turning angle, and the obstacle avoidance point-set optimization algorithm is used to find the optimal obstacle avoidance point in the obstacle avoidance point set. Through this algorithm framework, the USV can achieve efficient obstacle avoidance and path planning in complex marine environments.

Design of Obstacle Avoidance Strategy
According to the calculation of d cpa , the maximum total time required for USV steering is t cpa ; steering of at least ∆η is required after t cpa time period to avoid dangerous areas for incoming ships and to pass at the safe closest encounter distance d sa f e . The speed window of the USV is mainly limited by the following three factors: 1.
Self maximum and minimum speed limits The USV has a safe speed limit, and not all speeds can be used for safe USV travel. Therefore, v min ,v max represents the safe speed interval range.

Speed limitations affected by motor performance
3. Sampled speed affected by an obstacle. Figure 6. OCAP algorithm framework.
In this paper, combined with the optimal collision avoidance point, we focus on improving the speed V A of the USV limited by obstacles. The classical dynamic window approach generally defines an adequate search space that conforms to the dynamic limit in the velocity space (v, ω). Still, the actual steering process of the USV is more based on the change of the heading angle ∆α to complete the vessel's collision avoidance behavior. Therefore, this paper replaces the state space formed by the velocity (v, ω) with the state (v, ∆α). According to the formula, the intersection V r of V s and V d represents the adequate state space of the ship in the next period.
Equation (22) indicates that a set of possible motion trajectories can be generated by sampling the velocity and direction of the USV. Then, we evaluate these trajectories based on their cost function to select the optimal one as the USV's motion plan. Specifically, V s is the velocity and direction sampling space, where v min and v max represent the minimum and maximum velocity the USV can reach and ∆ψ represents the angle range the USV can rotate. V d represents the velocity and direction range the USV can reach while avoiding collisions, v c is the current velocity,v a andv b are the acceleration and deceleration, respectively, ∆t is the sampling-time interval, and ∆η max represents the maximum angle the USV can rotate. V A represents the velocity and direction range the USV can reach while maintaining a certain acceleration and turning speed, v 0 is the initial velocity of the USV, and∆η max is the maximum angular velocity the USV can rotate. Finally, V is the intersection of the three sampling spaces, representing all possible combinations of velocity and direction that the USV can sample. By evaluating these combinations based on their cost functions, the algorithm selects the optimal motion plan to achieve the goal of collision avoidance.
In Algorithm 2, the main calculation task is to perform obstacle avoidance on the sampling points and find the optimal collision avoidance point. This task is completed by two functions, Generate OCAP (Algorithm 1) and dynamic_obstacle_avoidance. Generate OCAP (Algorithm 1) is responsible for generating a set of sampling points, and the dy-namic_obstacle_avoidance function is responsible for performing obstacle avoidance on the sampling points. In these two functions, the algorithm uses sensor data and environmental information to calculate the trajectory of the unmanned boat and adjust its heading and speed based on the position and velocity of obstacles.

Parameter Selection
Three algorithms-dynamic window approach (DWA), dynamic window approach with virtual manipulators (DWV) [36], and OCAP-are considered in this study, where DWV and DWA are considered conventional methods and OCAP is envisioned as the proposed improved method. The simulations show the quality of the operation of such an algorithm. This paper observed a collision avoidance simulation using a minimum safety domain R for vessels. Moreover, the influence of navigational behaviors and environmental impacts (wind and currents) are ignored in the modeling process. All the algorithms in this study run on MATLAB 2020b. In this paper, two cases are considered: a constant map and a random map. These maps' size for all cases is 20 × 20. Case1 is simulated once, and Case2 is simulated 100 times. The start and goal positions of all cases are (USVxstart, USVystart) = (2.0, 6.0) and (GBxgoal, GBygoal) = (19.0, 18.0). When the distance between the USV and the goal position is less than 0.3 m, it is judged to have reached the goal. The other parameters of the simulation experiment are set as shown in Table 1. As shown in Figure 7, there are 12 static obstacles, which is a static obstacle density of 0.04, in the simulation environment. The solid circle is the expanded range with the longest radius of the obstacle, and the dashed circle is the safe area, where the d sa f e value of the obstacle relative to the USV is the radius. According to Equation (13), d sa f e is only related to the size of the obstacle, which is stationary. When the obstacle moves, it is also related to the relative velocity of the USV and target vessel. The simulation cases are defined as follows. Combining Algorithms 1 and 2, the time complexity of our proposed algorithm is O(n 3 ), where n represents the size of the input data. The algorithm consists of three nested loops and a recursive call. The time complexity of the first loop is O(n), of the second loop is O(n 2 ), and of the third loop is O(n 3 ). The time complexity of the recursive call is O(logn). Therefore, the total time complexity of the algorithm is O(n 3 + logn). Additionally, the space complexity of the algorithm is O(n) because it requires storing a copy of the input data in memory as well as intermediate results of multiple recursive calls.

Simulation Results and Discussion
As shown in Figure 7a Table 2 displays simulation results for Case1, including the success rate, algorithm time consumption (Time), trajectory length (TL), and λ dis in achieving the goal without collisions, and the average travel time. OCAP reached the goal position earlier than DWA and DWV. From Figure 7b and Table 2, the best result in Case1 was obtained by OCAP, which did not need to evaluate the optimal collision points and moved at the maximum translational velocity on the optimal path. Comparing static obstacle generation paths, the efficiency and results of the OCAP and DWV algorithms are similar. However, DWA sometimes generates a 'circling' motion when avoiding obstacles to reach a specified location. Therefore, the target time of DWA is longer than those of the other two algorithms.  Figure 8a shows simulation environments of the random maps in Case2 at a static and dynamic obstacle density of 0.04, which presents five static obstacles and seven dynamic vessels from different directions (considered dynamic obstacles), which are 12 obstacles in total. These obstacles are placed in random positions and given random velocities that are lower than the maximum velocity of the USV. The velocities of obstacles are set randomly in the range of 0.0 (m/s) to 0.2 (m/s). Case2 is simulated 100 times. Figure 8b shows path-finding results for a certain time.   Figure 9a, OCAP generated path candidates considering dynamic obstacles. OCAP also considered dynamic blocks when the optimal path was selected from path candidates. Thus, OCAP reached the goal position. Overall, the path collision hazard probability obtained by the OCAP algorithm is low. The optimal obstacle avoidance trajectory results with the lowest probabilities of collision avoidance are displayed in Figure 9b,c. DWV and DWA generated path candidates without considering dynamic obstacles. When DWA reached the goal position of avoiding moving obstacles, DWA sometimes generated backward movements. From Figure 9 and Table 3, OCAP has the highest success rate of reaching the goal at 82.73% along with the longest path-finding time and the lowest level of risk rate at 18.09% in Case2. These three results are correlated. As circling was sometimes generated to avoid the obstacle, the goal time of OCAP was longer than that of other methods. Although the time cost of the OCAP and DWV is similar, the path planned by OCAP is less likely to have collided and thus has a lower risk rate.  Figure 10 shows the evolution of four moving features during simulation. From Figure 10a,b, the obstacles are static in Case1, from which it can be seen that in the proposed new algorithm OCAP, the changes to the heading angle and course angle are minor, which means that the vessel does not have rapid turns or emergency braking during handling. The speed and t cpa shown in Figure 10c,d are the two most critical parameters reflecting the USV's state of avoiding obstacles. In the new algorithm OCAP, the t cpa of USV always appears within the range of change and increases linearly, indicating that the risk of collision avoidance of the USV in this algorithm is always in the acceptable range. Specifically, according to Figure 10d, in the OCAP algorithm, the minimum encounter time t cpa is always greater than 0, indicating that it is effective for path planning to choose the best collision avoidance point.
The results of changing the density of obstacles in the random map and testing multiple sets of data are shown in Table 4, which includes the success rate, algorithm time consumption (T), trajectory length (TL), risk rate (λ dis ) in achieving the goal without collisions, and the average travel time.  From Table 4, when the obstacle density is smallest (0.02), the path-finding success rates of all three algorithms are high and not much different; however, as the density of random map obstacles increases, the path-finding success rates of all three algorithms decrease, among which the decrease rate of the DWV algorithm changes the most. This is because the DWV algorithm is more sensitive to the number of obstacles: the more obstacles, the more DWV demonstrates the characteristics of a breadth-first algorithm that will fail to complete the computation in the specified time, making the sharpest decline in the success rate.
Comparing the computation time of the three algorithms, the OCAP algorithm consumes less time to generate dynamic discrete points than dynamic path generation because the optimal collision avoidance points are generated in advance before the path is generated. At the maximum obstacle density (0.08), the path-finding success rate of all three algorithms decreases greatly. However, since the OCAP algorithm generates dynamic discrete points, if the path changes (e.g., previously unmeasured dynamic obstacles) before driving the generated path, only the best collision avoidance point needs to be measured again, thus effectively improving the path-finding success rate. It can also be seen that the length of the path generated by the three algorithms is about the same when the density is large, and the main difference lies in the success rate of generation and the time consumed by the algorithm.
From Figure 11, the path-planning performance of three different algorithms on three cases is analyzed as a whole. In two other cases, the path planned by the OCAP algorithm has the shortest length and the least time cost. OCAP has the highest success rate of reaching the goal at 100%. The best result in Case2 was obtained by OCAP, which shows that this algorithm is more suitable for avoiding low-speed dynamic obstacles. Figure 11. Three algorithms in optimal path-planning performance: (a) total path-planning time (b) total path-planning length.

Conclusions
In this paper, OCAP, a new collision avoidance algorithm, is proposed. OCAP generated obstacle-avoidable path candidates. Path candidates were generated using the optimal collision avoidance point based on predictions of static and dynamic obstacles. Kinematics and dynamics constraints were taken into account in OCAP. The paper used simulations and experiments, demonstrating the proposed method to be effective. Even when the obstacle density increases, the effectiveness of trajectory generation is ensured because the OCAP algorithm can effectively and dynamically evaluate the minimum obstacle avoidance distance. Through simulation experiments, the algorithm was shown to be more suitable for high-density environments, and by evaluating the optimal collision avoidance points, the generated paths can be kept away from the obstacles over a larger area. The results of this study are limited to situations based on ship encounters in the calm water conditions considered in this study. Additionally, no consideration was made for hull-to-hull interaction and hull-propeller-rudder-engine interaction between the two vessels, which is a direction for future research.

Conflicts of Interest:
The authors declare no conflict of interest.